#ifndef _ENTROPY_MAP_H
#define _ENTROPY_MAP_H

#include "plan_env/grid_map.h"
#include "plan_env/polygon_intersector.hpp"

/* Constant parameters */
struct EntropyMapParameters
{
  /* entropy map bound (pixel)*/
  int entropy_x_bound_, entropy_y_bound_;
  /* max velocity of object in enviroment m/s */
  double env_vel_max_;
  double env_vel_max_voxel_;
  // TODO 可以和最大视野长度与最大环境中物体移动速度结合
  /* Time for entropy to change from order to disorder */
  double entropy_inc_time_;
  /* A factor used in fusing the traj into the entropy map*/
  double traj_attract_factor_;
  /* safe distance */
  double safe_dis;
  /**
   * The max R2 of dynamic obstacle to be considered
   * R2 is calculated by prediction, represents the goodness of fit */
  double dyn_obs_R2_max_;
  /* factor for dynamic obstacle in calculating entropy */
  double lambda_dyn_obs_;
  /**
   * if dynamic obstacle is dangerous,
   * the factor will be added by this */
  double dyn_attention_gain_;
  /* the max size of dynamic buffer(in time dimension) */
  size_t max_dyn_buffer_size{5};
};

/* Intermediate mapping data */
struct EntropyMapData
{
  /* map buffer */
  std::vector<double> entropy_time_buffer_;

  /* dynamic obstacle buffer */
  std::vector<std::vector<double>> dyn_obs_buffer_;
  std::vector<double> dyn_time_buffer_;
  size_t dyn_layers_{0};
  size_t dyn_used_layers_{0};
  size_t dyn_index0, dyn_index1;
  double dyn_interpolate_coeff0, dyn_interpolate_coeff1;

  ros::Time start_time_;
  double cur_time_;

  /* trajectory buffer */
  std::vector<double> traj_buffer_;
  double traj_update_time_; // time for last traj update, global time
  double traj_duration_;    // duration of the traj

  /* flag */
  bool entropy_need_update_;

  /* the min depth in a col of depth image */
  vector<Eigen::Vector3d> pts_bound_;
  int pts_bound_cnt_;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

class EntropyMap : public GridMap
{
public:
  EntropyMap() {}
  virtual ~EntropyMap() {}

  PolygonIntersector pi_;

  virtual void initMap(ros::NodeHandle &nh);
  void publishEntropy();
  void publishTrajMap();
  void publishDynamicObs(bool isEmpty = false);

  bool getEntroy(Eigen::Vector3d pos, double theta, double delta,
                 int step_cnt, std::vector<double> &result);
  bool getEntroy(Eigen::Vector3d pos, Eigen::Vector3d vel,
                 ros::Time t_plan,
                 std::vector<double> angles,
                 std::vector<double> &result);

  void updateTrajMap(std::vector<Eigen::Vector3d> pos_3,
                     std::vector<double> t_cur,
                     double duration,
                     double start_time_global);
  void resetTrajMap();

  void updateDynamicObs(const std::vector<std::vector<Eigen::Vector3d>> &pos,
                        const std::vector<std::vector<double>> &R2,
                        const std::vector<std::vector<double>> &times,
                        const std::vector<bool> &isdanger);
  void resetDynamicObs();

  double getCamYaw();

  typedef std::shared_ptr<EntropyMap> Ptr;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

protected:
  EntropyMapParameters emp_;
  EntropyMapData emd_;

  ros::Publisher map_entropy_pub_, map_traj_info_pub_, map_dyn_obs_pub_;
  ros::Timer entropy_timer_;

  void visCallback(const ros::TimerEvent & /*event*/);

  void updateEntropyCallback(const ros::TimerEvent &);

  void projectDepthImage();

  double calEnvEntropy(double cur_time, int id);
  double calTrajCost(double cur_time, int id);
  double calDistanceCost(const Eigen::Vector3i &index);
  double calDynamicObsEntropy(int id);

  double getEntroyAlongLine(double cur_time, Eigen::Vector3d pos, double theta,
                            bool &isBlock, Eigen::Vector2i &block_idx);

  void preProcess(double t_global);
};

/* -------------------------------------------------------------------------- */
/*                        definition of inline function                       */
/* -------------------------------------------------------------------------- */

/* Get the Z-axis of camera, project into XY-Plane in world frame */
inline double EntropyMap::getCamYaw() { return atan2(gmd_.camera_r_m_(1, 2),
                                                     gmd_.camera_r_m_(0, 2)); };

/* Calculate the entropy of a point at current time (0~1)*/
inline double EntropyMap::calEnvEntropy(double cur_time, int id)
{
  double entropy = 1.0;
  double time_diff = cur_time - emd_.entropy_time_buffer_[id];
  if (time_diff < emp_.entropy_inc_time_)
  {
    entropy = time_diff / emp_.entropy_inc_time_;
  }
  return entropy;
}

/* Calculate the trajectory cost of a point at current time (0~1) */
inline double EntropyMap::calTrajCost(double cur_time, int id)
{
  double cost = 1.0;
  double time_diff = cur_time - emd_.entropy_time_buffer_[id];
  if (time_diff < emp_.entropy_inc_time_)
  {
    cost = time_diff / emp_.entropy_inc_time_;
  }
  return cost;
}

/* Calculate the distance cost of a point (0~1) */
inline double EntropyMap::calDistanceCost(const Eigen::Vector3i &index)
{
  double cost =
      1 - gmd_.distance_buffer_[toAddress(index)] / emp_.safe_dis;
  return cost > 0 ? cost : 0;
}

/**
 * @brief use linear interpolation to calculate
 *        the dynamic obstacle cost
 */
inline double EntropyMap::calDynamicObsEntropy(int id)
{
  double cost = 0.0;
  if (emd_.dyn_used_layers_ == 0) {
    return 0.0;
  }
  else if (emd_.dyn_used_layers_ == 1) {
    return emd_.dyn_obs_buffer_[emd_.dyn_index0][id] *
           emd_.dyn_interpolate_coeff0;
  }
  else {
    return emd_.dyn_obs_buffer_[emd_.dyn_index0][id] *
               emd_.dyn_interpolate_coeff0 +
           emd_.dyn_obs_buffer_[emd_.dyn_index1][id] *
               emd_.dyn_interpolate_coeff1;
  }
  return 0.0;
}

inline void EntropyMap::preProcess(double t_global)
{
  // for dynamic obstacle buffer
  if (emd_.dyn_layers_ == 0)
  {
    emd_.dyn_used_layers_ = 0;
  }
  else {
    size_t i = 0;
    for (; i < emd_.dyn_time_buffer_.size(); i++)
    {
      if (t_global < emd_.dyn_time_buffer_[i])
      {
        if (i == 0)
        {
          emd_.dyn_used_layers_ = 1;
          emd_.dyn_index0 = 0;
          emd_.dyn_interpolate_coeff0 = 1;
        }
        else {
          emd_.dyn_used_layers_ = 2;
          emd_.dyn_index0 = i - 1;
          emd_.dyn_index1 = i;
          emd_.dyn_interpolate_coeff1 =
              (t_global - emd_.dyn_time_buffer_[i - 1]) /
              (emd_.dyn_time_buffer_[i] - emd_.dyn_time_buffer_[i - 1]);
          emd_.dyn_interpolate_coeff0 = 1 - emd_.dyn_interpolate_coeff1;
        }
        break;
      }
    }
    if (i == emd_.dyn_time_buffer_.size())
    {
      emd_.dyn_used_layers_ = 1;
      emd_.dyn_index0 = emd_.dyn_time_buffer_.size() - 1;
      emd_.dyn_interpolate_coeff0 = 1;
    }
  }
}

#endif